package CamposDePotencial;

import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.IOException;
import java.io.InputStream;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.Properties;

import javax.vecmath.Point3d;

import comunicacion.ParametrosEntorno;

import CamposDePotencial.FabricaPotenciales.PAREDES;

import estrategia.ParametrosGlobales;
import estrategia.ParametrosPotenciales;
import estrategia.Utiles;
import estrategia.Vector;

public class CampoSemiplano extends CampoPotencial {

//	private final double PARED_IZQ = 0.02;
//	private final double PARED_ABAJO = 0.02;
//	private final double  PARED_DER = 2.2;
//	private final double PARED_ARRIBA = 1.78;
	
	private double radioCampo;
	private double tamanioRobot;
	private double Krepulsion;
	
	private double angulo;
	private Point3d pared;
	
	private ArrayList<CampoPotencial> camposObstaculos;
	private final double SESGO_FUERZA_PARED = 10;
	
	
	public CampoSemiplano(PAREDES p, ArrayList<CampoPotencial> camposObstaculos, Utiles utiles) {
		// TODO Auto-generated constructor stub
		super(0, utiles, null);
		// angulo depende de las paredes
		switch(p) {
			case ARRIBA:
				angulo = -Math.PI/2;
				//System.out.println("CREADO CAMPO ARRIBA");
				break;
			case ABAJO:
				angulo = Math.PI/2;
				//System.out.println("CREADO CAMPO ABAJO");
				break;
			case IZQ:
				angulo = 0;
				System.out.println("CREADO CAMPO IZQ");
				break;
			case DER:
				angulo = Math.PI;
				//System.out.println("CREADO CAMPO DER");
				break;
		}
		
		this.camposObstaculos = camposObstaculos;
	}
	
	public void setCamposObstaculos(ArrayList<CampoPotencial> camposObstaculosNuevos) {
		camposObstaculos = camposObstaculosNuevos;
	}
	
	public void  cargarConstantes() {
		
		radioCampo = ParametrosPotenciales.radioCampoSemiplano;
		Krepulsion = ParametrosPotenciales.KCampoSemiplano;
		tamanioRobot = ParametrosPotenciales.tamanioRobot;
	}
	
	
	public Point3d getPared(Point3d p) {
		if (angulo == 0) 
			return new Point3d(ParametrosEntorno.POSICION_PARED_IZQUIERDA, p.y, 0);
		else if (angulo == Math.PI/2) 
			return new Point3d(p.x, ParametrosEntorno.POSICION_PARED_TRASERA, 0);
		else if (angulo == Math.PI) 
			return new Point3d(ParametrosEntorno.POSICION_PARED_DERECHA, p.y, 0);
		else 
			return new Point3d(p.x, ParametrosEntorno.POSICION_PARED_DELANTERA, 0);
		
	}
	
	
	public double getMagnitudFuerzaObstaculos(Point3d mipos) throws IOException {
		Vector resultante = new Vector (0,0);
		
		// recorro la coleccion de campos que corresponde a obstaculos y hallo la magnitud de todos esos campos
		Iterator<CampoPotencial> iter = camposObstaculos.iterator();
		while (iter.hasNext()) {
			CampoPotencial campo = iter.next();
			Vector fCampo = campo.obtenerFuerza(mipos);
			resultante = utiles.sumaVectores(resultante, fCampo);
		}
		
		return Math.abs(resultante.getMagnitud());
	}
	
	public double getMagnitudFuerza(Point3d mipos) throws IOException {
		double magnitud = 0;
		pared = getPared(mipos);
		double distancia = utiles.distanciaEuclidea(mipos, pared);
		//distancia = distancia - tamanioRobot;
		
		//System.out.println("La distancia a la pared es: " + distancia);
		if (distancia <= radioCampo) {
			// *(1/(Math.pow(distancia, 2)))
			double fuerzaObstaculos = getMagnitudFuerzaObstaculos(mipos);
			//System.out.println("La fuerza de los obstaculos es: " + fuerzaObstaculos);
			//magnitud = Krepulsion*((1/distancia) - (1/radioCampo));
			magnitud = fuerzaObstaculos + SESGO_FUERZA_PARED;
		}
		
		return magnitud;
	}
	
	public Vector obtenerFuerza(Point3d mipos){
		
		cargarConstantes();
		
		double direccion = angulo;
		
		double magnitud = 0;
		try {
			magnitud = getMagnitudFuerza(mipos);
		} catch (IOException e) {
			// TODO Auto-generated catch block
			System.err.println("Error al calcular la magnitud del Campo Semiplano");
		}
		 
		Vector resultante = new Vector(Math.abs(magnitud), direccion);
		
		//System.out.println("La magnitud del SEMIPLANO " + angulo + " es: " + resultante.getMagnitud() + " y la direccion es: " + resultante.getDireccion());
		return resultante;
	};

}
